package com.gitee.wsl.mathematics.geometry.d3.plane

import com.gitee.wsl.ext.number.isFuzzyEqual
import com.gitee.wsl.ext.number.isFuzzyZero
import com.gitee.wsl.mathematics.coordinate.d3.Coordinate3D
import com.gitee.wsl.mathematics.coordinate.d3.Point3D
import com.gitee.wsl.mathematics.geometry.d3.vertex.Vertex
import com.gitee.wsl.mathematics.coordinate.ext.crossCentered
import com.gitee.wsl.mathematics.vector.ext.cross
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.vector.ext.plusAssign
import com.gitee.wsl.mathematics.vector.vec3.MutableVec3
import com.gitee.wsl.mathematics.vector.vec3.Vec3
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.mathematics.vector.vec3.Vector3D

interface Plane: Coordinate3D<Vec3> {
    val d: Double
}

open class MutablePlane(
    x: Double,
    y: Double,
    z: Double,
    override var d: Double
) : Plane, MutableVec3(x, y, z) {
    constructor(p: Plane) : this(p.x, p.y, p.z, p.d)
    override fun toString(): String =
        "Plane(n=${super.toString()}, d=${d})"
}

// Note: n must be a unit vector
fun Plane(n: Vector3<Double, *>, d: Double): Plane =
    MutablePlane(n.x, n.y, n.z, d)

// Plane through a point with a given normal
fun planeByNormalAndPoint(n: Coordinate3D<*>, p: Coordinate3D<*>): Plane {
    val u = n.unit
    return Plane(u, p dot u)
}

// Plane through 3 points
fun plane3(a: Vec3, b: Vec3, c: Vec3): Plane =
    planeByNormalAndPoint(((c - a) cross (b - a)), a)

// Intersect 3 planes
fun planeIntersection(p: Plane, q: Plane, r: Plane): Vec3 {
    val d = det(
        p.x, p.y, p.z,
        q.x, q.y, q.z,
        r.x, r.y, r.z
    )
    val x = det(
        p.d, p.y, p.z,
        q.d, q.y, q.z,
        r.d, r.y, r.z
    )
    val y = det(
        p.x, p.d, p.z,
        q.x, q.d, q.z,
        r.x, r.d, r.z
    )
    val z = det(
        p.x, p.y, p.d,
        q.x, q.y, q.d,
        r.x, r.y, r.d
    )
    return Vec3(x / d, y / d, z / d)
}

operator fun Plane.contains(v: Point3D): Boolean =
    this.dot(v).isFuzzyEqual(d)

// Projection of origin onto the plane
val Plane.tangentPoint: Vec3
    get() = this * d

// Intersection of a plane with a given vector
// Resulting vector is in the plane
fun Plane.intersection(v: Vec3) =
    v * (d / (this dot v))

// Average plane via given points, outside pointing normal
fun List<Vertex>.averagePlane(): Plane {
    require(size >= 3) { "Needs at least 3 points, found $size" }
    val center = MutableVec3()
    // find centroid of points
    for (element in this) center += element
    center /= size
    // find sum cross-product of all angles -> normal of the "average" plane
    val normSum = MutableVec3()
    for (i in indices) {
        val a = this[i]
        val b = this[(i + 1) % size]
        normSum.crossCentered(b, a, center)
    }
    return planeByNormalAndPoint(normSum, center)
}

fun det(
    m11: Double, m12: Double,
    m21: Double, m22: Double
): Double =
    m11 * m22 - m12 * m21

fun det(
    m11: Double, m12: Double, m13: Double,
    m21: Double, m22: Double, m23: Double,
    m31: Double, m32: Double, m33: Double
): Double =
    m11 * det(m22, m23, m32, m33) -
            m21 * det(m12, m13, m32, m33) +
            m31 * det(m12, m13, m22, m23)

infix fun Vec3.approx(u: Vec3): Boolean =
    x.isFuzzyZero(u.x)  && y.isFuzzyZero( u.y) && z.isFuzzyZero(u.z)

fun Plane.dualPoint(r: Double) =
      this * (r * r / d)

fun Vector3D<*>.dualPlane(r: Double): Plane {
    val n = length
    return Plane(this / n, r * r / n)
}